#include "core.h"


namespace core{

    
    // 获取TF坐标变换
    bool GetTFPose(tf2_ros::Buffer* tf_buffer, geometry_msgs::PoseStamped& global_pose, string father_frame, string child_frame, ros::Time stamp){
        tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
        geometry_msgs::PoseStamped robot_pose;
        tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
        robot_pose.header.frame_id = child_frame;
        robot_pose.header.stamp = ros::Time();
        // get the global pose of the robot
        try
        {
            // use current time if possible (makes sure it's not in the future)
            if (tf_buffer->canTransform(father_frame, child_frame, stamp))
            {
                geometry_msgs::TransformStamped transform = tf_buffer->lookupTransform(father_frame, child_frame, stamp);
                tf2::doTransform(robot_pose, global_pose, transform);
            }
            // use the latest otherwise
            else
            {
                tf_buffer->transform(robot_pose, global_pose, father_frame);
            }
        }
        catch (tf2::LookupException& ex)
        {
            ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
            return false;
        }
        catch (tf2::ConnectivityException& ex)
        {
            ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
            return false;
        }
        catch (tf2::ExtrapolationException& ex)
        {
            ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
            return false;
        }
        // // check global_pose timeout
        // ros::Time current_time = ros::Time::now();
        // if(current_time.toSec() - global_pose.header.stamp.toSec() > __transform_tolerance)
        // {
        //     ROS_WARN_THROTTLE(1.0,
        //                     "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
        //                     current_time.toSec(), global_pose.header.stamp.toSec(), __transform_tolerance);
        //     return false;
        // }

        return true;        
    }

}// namespace

